#include "Physics.h"
#include <cmath>

float Physics::epsilon;

Physics::Physics()
{
	sphereMap = NULL;
	terrainMap = NULL;
	borderWidth = 0;
	epsilon = 0.000001f;
	collisionHandler = NULL;
	cameraSphere = NULL;
}

Vector3 Physics::GetBoundedVelocity(Vector3 &normal, Vector3 &velocity)
{
	Vector3 v = -1.0f * velocity;
	if(v.Length() < epsilon)
		return 0.001f * normal;
	v.Normalize();
	return velocity + 2.0f * normal * velocity.Length() * Vector3::DotProduct(normal, v);
}

Vector3 Physics::GetTouchPosition(Vector3 &mirrorNormal, Vector3 &mirrorPoint, Vector3 &point)
{	
	Vector3 v = point - mirrorPoint;
	Vector3 vNormlized = v;
	if(v.Length() < epsilon)
		return point + 0.0001f * mirrorNormal;
	vNormlized.Normalize();	
	float a = Vector3::DotProduct(vNormlized, mirrorNormal);
	return point - touchDistanceMultiplyer * a * mirrorNormal * v.Length() + 0.0001f * mirrorNormal;
}

void Physics::MaintainBoundingBoxCollision(DynamicSphere *sphere)
{	
	Vector3 position = sphere->Position();
	Vector3 v, plainPoint, normal;
	for(int i = 0; i < 6; i++)
	{
		normal = boundingPlanes[i].Normal();
		plainPoint = boundingPlanes[i].Point() + normal * DynamicSphere::Radius();
		v = position - plainPoint;
		if(Vector3::DotProduct(v, normal) < 0)	//collision
		{
			position = GetTouchPosition(normal, plainPoint, position);			
			Vector3 velocity = sphere->Velocity();
			sphere->SetVelocity(GetBoundedVelocity(normal, velocity));
		}
	}
	sphere->SetPosition(position);
}

void Physics::BoxMaintainVertexCollision(Vector3& vertex, DynamicSphere* sphere)
{
	Vector3 position = sphere->Position();
	Vector3 v = position - vertex;
	float distance = v.Length();
	if(distance < DynamicSphere::Radius())	
	{
		if(v.Length() < epsilon)			
			return;
		v.Normalize();
		position = vertex + v * (DynamicSphere::Diameter() - distance);		
		sphere->SetPosition(position);
		sphere->SetVelocity(GetBoundedVelocity(v, sphere->Velocity()));
	}
}

void Physics::BoxMaintainEdgeCollision(Vector3& edgePoint, Vector3& edgeDirection, DynamicSphere* sphere)
{	
	Vector3 position = sphere->Position();
	Vector3 v = position - edgePoint;
	if(v.Length() < epsilon)
		return;
	Vector3 vNormalized = v;
	vNormalized.Normalize();

	Vector3 nearestPoint = edgePoint + edgeDirection * v.Length() * Vector3::DotProduct(edgeDirection, vNormalized);
	v = position - nearestPoint;	
	float distance = v.Length();
	if(distance < DynamicSphere::Radius())	
	{	
		if(v.Length() < epsilon)
			return;
		vNormalized = v;
		vNormalized.Normalize();
		position = nearestPoint + vNormalized * (DynamicSphere::Diameter() - distance);
		sphere->SetPosition(position);
		sphere->SetVelocity(GetBoundedVelocity(vNormalized, sphere->Velocity()));
	}
}

bool Physics::MaintainBoxCollision(DynamicSphere *sphere, Box *box)
{	
	Vector3 position = sphere->Position();
	float normalBorder = box->Width() * 0.5f;
	float extendedBorder = normalBorder + DynamicSphere::Radius();	
	if(position.x < box->X() - extendedBorder)
		return false;
	if(position.x > box->X() + extendedBorder)
		return false;
	if(position.z < box->Y() - extendedBorder)
		return false;
	if(position.z > box->Y() + extendedBorder)
		return false;
	if(position.y > box->Height() + DynamicSphere::Radius())
		return false;

	float distance, boxX, boxY, boxZ;
	boxY = box->Height();
	if(position.x > (boxX = box->X() + normalBorder))
	{				
		if(position.z > (boxZ = box->Y() + normalBorder))
		{			
			if(position.y > boxY)
			{				
				BoxMaintainVertexCollision(Vector3(boxX, boxY, boxZ), sphere);
				return true;
			}						
			BoxMaintainEdgeCollision(Vector3(boxX, 0, boxZ), Vector3(0, 1, 0), sphere);
			return true;			
		}
		else if(position.z < (boxZ = box->Y() - normalBorder))
		{
			if(position.y > boxY)
			{
				BoxMaintainVertexCollision(Vector3(boxX, boxY, boxZ), sphere);
				return true;				
			}			
			BoxMaintainEdgeCollision(Vector3(boxX, 0, boxZ), Vector3(0, 1, 0), sphere);
			return true;							
		}
		if(position.y > boxY)
		{
			BoxMaintainEdgeCollision(Vector3(boxX, boxY, boxZ), Vector3(0, 0, 1), sphere);
			return true;
		}
		sphere->SetPosition(GetTouchPosition(Vector3(1, 0, 0), Vector3(box->X() + extendedBorder, 0, box->Y()), position));
		sphere->SetVelocity(GetBoundedVelocity(Vector3(1, 0, 0), sphere->Velocity()));
		return true;
	}
	else if(position.x < (boxX = box->X() - normalBorder))
	{				
		if(position.z > (boxZ = box->Y() + normalBorder))
		{			
			if(position.y > boxY)
			{				
				BoxMaintainVertexCollision(Vector3(boxX, boxY, boxZ), sphere);
				return true;
			}						
			BoxMaintainEdgeCollision(Vector3(boxX, 0, boxZ), Vector3(0, 1, 0), sphere);
			return true;			
		}
		else if(position.z < (boxZ = box->Y() - normalBorder))
		{
			if(position.y > boxY)
			{
				BoxMaintainVertexCollision(Vector3(boxX, boxY, boxZ), sphere);
				return true;				
			}			
			BoxMaintainEdgeCollision(Vector3(boxX, 0, boxZ), Vector3(0, 1, 0), sphere);
			return true;
		}
		if(position.y > boxY)
		{
			BoxMaintainEdgeCollision(Vector3(boxX, boxY, boxZ), Vector3(0, 0, 1), sphere);
			return true;
		}
		sphere->SetPosition(GetTouchPosition(Vector3(-1, 0, 0), Vector3(box->X() - extendedBorder, 0, box->Y()), position));
		sphere->SetVelocity(GetBoundedVelocity(Vector3(-1, 0, 0), sphere->Velocity()));
		return true;
	}
	if(position.z > (boxZ = box->Y() + normalBorder))
	{			
		if(position.y > boxY)
		{				
			BoxMaintainEdgeCollision(Vector3(boxX, boxY, boxZ), Vector3(1, 0, 0), sphere);
			return true;			
		}		
		sphere->SetPosition(GetTouchPosition(Vector3(0, 0, 1), Vector3(boxX, 0, box->Y() + extendedBorder), position));
		sphere->SetVelocity(GetBoundedVelocity(Vector3(0, 0, 1), sphere->Velocity()));
		return true;
	}
	if(position.z < (boxZ = box->Y() - normalBorder))
	{			
		if(position.y > boxY)
		{				
			BoxMaintainEdgeCollision(Vector3(boxX, boxY, boxZ), Vector3(1, 0, 0), sphere);
			return true;			
		}		
		sphere->SetPosition(GetTouchPosition(Vector3(0, 0, -1), Vector3(boxX, 0, box->Y() - extendedBorder), position));
		sphere->SetVelocity(GetBoundedVelocity(Vector3(0, 0, -1), sphere->Velocity()));
		return true;
	}
	if(position.y > boxY)
	{
		sphere->SetPosition(GetTouchPosition(Vector3(0, 1, 0), Vector3(boxX, boxY + DynamicSphere::Radius(), boxZ), position));
		sphere->SetVelocity(GetBoundedVelocity(Vector3(0, 1, 0), sphere->Velocity()));
		return true;
	}

	//Deep inside box
	Vector3 nearestPoint;
	Vector3 normal;
	float minDistance = 100;
	Vector3 p = GetTouchPosition(Vector3(1, 0, 0), Vector3(box->X() + extendedBorder, 0, 0), position);
	if((distance = (p - position).Length()) < minDistance)
	{
		minDistance = distance;
		nearestPoint = p;
		normal = Vector3(1, 0, 0);
	}
	p = GetTouchPosition(Vector3(-1, 0, 0), Vector3(box->X() - extendedBorder, 0, 0), position);
	if((distance = (p - position).Length()) < minDistance)
	{
		minDistance = distance;
		nearestPoint = p;
		normal = Vector3(-1, 0, 0);
	}
	p = GetTouchPosition(Vector3(0, 0, 1), Vector3(0, 0, box->Y() + extendedBorder), position);
	if((distance = (p - position).Length()) < minDistance)
	{
		minDistance = distance;
		nearestPoint = p;
		normal = Vector3(0, 0, 1);
	}
	p = GetTouchPosition(Vector3(0, 0, -1), Vector3(0, 0, box->Y() - extendedBorder), position);
	if((distance = (p - position).Length()) < minDistance)
	{
		minDistance = distance;
		nearestPoint = p;
		normal = Vector3(0, 0, -1);
	}
	p = GetTouchPosition(Vector3(0, 1, 0), Vector3(0, box->Height() + DynamicSphere::Radius(), 0), position);
	if((distance = (p - position).Length()) < minDistance)
	{
		minDistance = distance;
		nearestPoint = p;
		normal = Vector3(0, 1, 0);
	}
	
	sphere->SetPosition(nearestPoint);
	sphere->SetVelocity(GetBoundedVelocity(normal, sphere->Velocity()));

	return true;
}

bool Physics::SphereMaintainSphereCollision(DynamicSphere* sphere, DynamicSphere* secondSphere)
{
	Vector3 v = secondSphere->Position() - sphere->Position();	
	float distance = v.Length();
	if(distance >= DynamicSphere::Diameter())
		return false;

	sphere->SetWasSphereCollisionMaintained(true);
	secondSphere->SetWasSphereCollisionMaintained(true);

	if(collisionHandler)
	{
		if(!collisionHandler->Collide(sphere, secondSphere))
			return false;
	}
	list<DynamicSphere*>* currentField, *secondField;
	
	Vector3 position = secondSphere->Position();
	secondField = SphereMap(position.x, position.y, position.z);
	if(v.Length() < epsilon)
	{
		sphere->SetPosition(sphere->Position() + 1000.0f * v);
		secondSphere->SetPosition(secondSphere->Position() - 1000.0f * v);
	}
	else
	{
		Vector3 vNormalized = v;
		vNormalized.Normalize();

		Vector3 v1 = vNormalized * Vector3::DotProduct(sphere->Velocity(), vNormalized);
		Vector3 v2 = -1.0f * vNormalized * Vector3::DotProduct(secondSphere->Velocity(), -1.0f * vNormalized);
			
		sphere->SetPosition(sphere->Position() - vNormalized * 0.5f * (DynamicSphere::Diameter() - distance));
		secondSphere->SetPosition(secondSphere->Position() + vNormalized * 0.5f * (DynamicSphere::Diameter() - distance));
		sphere->SetVelocity(sphere->Velocity() - v1 + v2);
		secondSphere->SetVelocity(secondSphere->Velocity() - v2 + v1);
	}	
		
	MaintainBoundingBoxCollision(secondSphere);
	v = secondSphere->Position();
	currentField = SphereMap(v.x, v.y, v.z);
	if(secondField != currentField)
	{
		SafeSphereRelocate(secondSphere, secondField, currentField);
		return true;
	}
	return false;
}

void Physics::MaintainSphereCollision(DynamicSphere* sphere, list<DynamicSphere*> *firstField)
{	
#if _DEBUG
	EnsureOnList(sphere, firstField);
#endif

	Vector3 position = sphere->Position();			

	list<DynamicSphere*>::iterator iterator;
	DynamicSphere *secondSphere;
	int x, y, z, i;
	static list<DynamicSphere*>* alredyMaintained[8];
	int maintainedCount = 0;	
	Vector3 p;
	list<DynamicSphere*>* currentField;

	// optimized loop (minimize multiplications)
	for(p.x = position.x - DynamicSphere::Diameter(), x = -1; x < 2; x++, p.x += DynamicSphere::Diameter())
	{		
		for(p.y = position.y - DynamicSphere::Diameter(), y = -1; y < 2; y++, p.y += DynamicSphere::Diameter())
		{			
			for(p.z = position.z - DynamicSphere::Diameter(), z = -1; z < 2; z++, p.z += DynamicSphere::Diameter())
			{					
				if(IsInsideScene(p.x, p.y, p.z))
				{
					currentField = SphereMap(p.x, p.y, p.z);
					bool skip = false;
					for(i = 0; i < maintainedCount; i++)
					{
						if(alredyMaintained[i] == currentField)
						{
							skip = true;
							break;
						}
					}					

					if(skip)
						continue;
					alredyMaintained[maintainedCount] = currentField;
					maintainedCount++;
					bool checkOnceMore = false;
					while(true)
					{
						checkOnceMore = false;
						for(iterator = currentField->begin(); iterator != currentField->end(); ++iterator)
						{													
							secondSphere = *iterator;
							if(secondSphere->WasSphereCollisionMaintained())												
								continue;							
							if(secondSphere != sphere)
							{
								if(SphereMaintainSphereCollision(sphere, secondSphere))
								{
									checkOnceMore = true;
									break;
								}
							}
						}
						if(!checkOnceMore)
							break;
					}
				}
			}
		}
	}
	
	MaintainBoundingBoxCollision(sphere);
	position = sphere->Position();
	currentField = SphereMap(position.x, position.y, position.z);
	if(firstField != currentField)
	{
		SafeSphereRelocate(sphere, firstField, currentField);
	}		
}

void Physics::Simulate(float time)
{
	if(sphereMap == NULL)
		return;

	DynamicSphere *currentSphere;
	list<DynamicSphere*>::iterator itSphere = spheres.begin();
	for(;itSphere != spheres.end(); ++itSphere)
		(*itSphere)->SetWasSphereCollisionMaintained(false);	

	touchDistanceMultiplyer = 2.0f;
	Vector3 position;
	list<DynamicSphere*>* firstField, *currentField;
	for(itSphere = spheres.begin(); itSphere != spheres.end(); ++itSphere)
	{
		currentSphere = (*itSphere);
		position = currentSphere->Position();
		firstField = SphereMap(position.x, position.y, position.z);
#if _DEBUG
		EnsureOnList(currentSphere, firstField);
#endif
		position += time * currentSphere->Velocity();
		currentSphere->SetPosition(position);
		//Collision detection
		touchDistanceMultiplyer = 1.0f;
		MaintainBoundingBoxCollision(currentSphere);
		position = currentSphere->Position();		
		currentField = SphereMap(position.x, position.y, position.z);
		if(firstField != currentField)
		{
			SafeSphereRelocate(currentSphere, firstField, currentField);
			firstField = currentField;
		}
#if _DEBUG
		EnsureOnList(currentSphere, currentField);
#endif

		if((position.x >= 0.0f) && ((int)position.x < terrainMap->Columns()) && (position.z >= 0.0f) && ((int)position.z < terrainMap->Rows()))
		{			
			touchDistanceMultiplyer = 2.0f;
			if(MaintainBoxCollision(currentSphere, &terrainMap->Field((int)position.x, (int)position.z)))
			{
				touchDistanceMultiplyer = 1.0f;
				MaintainBoundingBoxCollision(currentSphere);
				position = currentSphere->Position();
				currentField = SphereMap(position.x, position.y, position.z);
				if(currentField != firstField)
				{
					SafeSphereRelocate(currentSphere, firstField, currentField);
					firstField = currentField;
				}
			}
		}
		touchDistanceMultiplyer = 2.0f;
		if(!currentSphere->WasSphereCollisionMaintained())		
			MaintainSphereCollision(currentSphere, firstField);		
	}

	//Camera Sphere	
	if(cameraSphere)
	{
		touchDistanceMultiplyer = 1.0f;
		MaintainBoundingBoxCollision(cameraSphere);	
		position = cameraSphere->Position();
		if((position.x >= 0.0f) && ((int)position.x < terrainMap->Columns()) && (position.z >= 0.0f) && ((int)position.z < terrainMap->Rows()))
			MaintainBoxCollision(cameraSphere, &terrainMap->Field((int)position.x, (int)position.z));
	}
}

void Physics::Init(Map *map)
{
	mapY = (int)ceilf(map->GetHighest()) + borderWidth;	
	mapX = map->Columns() + 2 * borderWidth;	
	mapZ = map->Rows() + 2 * borderWidth;	

	sphereMap = new list<DynamicSphere*> [mapX * mapY * mapZ];
	terrainMap = map;	

	boundingPlanes[0].SetPoint(Vector3(0, 0, 0));
	boundingPlanes[0].SetNormal(Vector3(0, 1, 0));
	boundingPlanes[1].SetPoint(Vector3(0, (float)mapY, 0));
	boundingPlanes[1].SetNormal(Vector3(0, -1, 0));
	boundingPlanes[2].SetPoint(Vector3((float)-borderWidth, 0, 0));
	boundingPlanes[2].SetNormal(Vector3(1, 0, 0));
	boundingPlanes[3].SetPoint(Vector3((float)(mapX - borderWidth), 0, 0));
	boundingPlanes[3].SetNormal(Vector3(-1, 0, 0));
	boundingPlanes[4].SetPoint(Vector3(0, 0, (float)(-borderWidth)));
	boundingPlanes[4].SetNormal(Vector3(0, 0, 1));
	boundingPlanes[5].SetPoint(Vector3(0, 0, (float)(mapZ - borderWidth)));
	boundingPlanes[5].SetNormal(Vector3(0, 0, -1));
}

void Physics::AddSphere(DynamicSphere* sphere)
{	
	Vector3 position;
	int x, y, z;
	position = sphere->Position();
	x = (int)position.x + borderWidth;
	y = (int)position.y;
	z = (int)position.z + borderWidth;

	if(x < 0)
	{
		x = 0;
		position.x = -borderWidth + DynamicSphere::Radius();
	}
	if(y < 0)
	{
		y = 0;
		position.y = DynamicSphere::Radius();
	}
	if(z < 0)
	{
		z = 0;
		position.z = -borderWidth + DynamicSphere::Radius();
	}

	if(x >= mapX)
	{
		x = mapX - 1;
		position.x = mapX - borderWidth - DynamicSphere::Radius();
	}
	if(y >= mapY)
	{
		y = mapY - 1;
		position.y = mapY - DynamicSphere::Radius();
	}
	if(z >= mapZ)
	{
		z = mapZ - 1;
		position.z = mapZ - borderWidth - DynamicSphere::Radius();
	}

	sphere->SetPosition(position);
	spheres.push_back(sphere);
	SphereMap(position.x, position.y, position.z)->push_back(sphere);	
}

Physics::~Physics()
{
	delete[] sphereMap;
	sphereMap = NULL;
	terrainMap = NULL;
}

list<DynamicSphere*>* Physics::SphereMap(float x, float y, float z)
{
	return &sphereMap[(int)x + borderWidth + mapX * ((int)y + mapY * ((int)z + borderWidth))];
}

bool Physics::IsInsideScene(float x, float y, float z)
{
	if((int)x + borderWidth < 0)
		return false;
	if(y < 0.0f)
		return false;
	if((int)z + borderWidth < 0)
		return false;
	if((int)x + borderWidth >= mapX)
		return false;
	if((int)y + borderWidth >= mapY)
		return false;
	if((int)z + borderWidth >= mapZ)
		return false;
	return true;
}

void Physics::SetCameraSphere(DynamicSphere* sphere)
{
	cameraSphere = sphere;
}

bool Physics::BoxIsBoxColliding(Box* box, Vector3 &position, float size)
{
	float boxBorder = 0.5f * Box::Width() + size;
	float x1 = box->X() + boxBorder;
	float x2 = box->X() - boxBorder;
	float z1 = box->Y() + boxBorder;
	float z2 = box->Y() - boxBorder;	
	float y1 = box->Height() + size;

	if(x1 < position.x)
		return false;
	if(x2 > position.x)
		return false;
	if(z1 < position.z)
		return false;
	if(z2 > position.z)
		return false;	
	if(y1 < position.y)
		return false;	
	if(position.y < -size)
		return false;
	return true;
}

bool Physics::BoxIsPlaneColliding(Plane* plane, Vector3 &position, float size)
{
	Vector3 v = plane->Point() + plane->Normal() * size;	
	v = position - v;
	if(Vector3::DotProduct(v, plane->Normal()) < 0.0f)
		return true;
	return false;
}

bool Physics::IsCollidingSetOfBoxes(Vector3 &a, Vector3 &b, float size)
{
	int i;

	//Test bounding box
	for(i = 0; i < 6; i++)
	{
		if(BoxIsPlaneColliding(&boundingPlanes[i], a, size))
			return true;
		if(BoxIsPlaneColliding(&boundingPlanes[i], b, size))
			return true;
	}

	//Test blocks
	Vector3 v = b - a;
	int count;
	float stepLength;
	Vector3 vNormalized;
	if(v.Length() < epsilon)
	{
		count = 1;
		stepLength = 0;
		vNormalized = Vector3(1, 0, 0);
	}
	else
	{
		vNormalized = v;
		vNormalized.Normalize();
		float length = v.Length();
		stepLength = 1.0f * size;
		count = 1 + (int)(length / stepLength);
		stepLength = length / count;
	}
	
	for(i = 0; i <= count; i++)
	{	
		v = a + vNormalized * stepLength * (float)i;
		if((v.x >= 0.0f) && ((int)v.x < terrainMap->Columns()) && (v.z >= 0.0f) && ((int)v.z < terrainMap->Rows()))
		{
			if(BoxIsBoxColliding(&terrainMap->Field((int)v.x, (int)v.z), v, size))
				return true;
		}
	}
	return false;
}

int Physics::GetSpheresCount()
{
	return (int)this->spheres.size();
}

int Physics::GetSphereMapCount()
{
	int count = 0;
	for (int x = -borderWidth; x < mapX - borderWidth; x++) 
	{
		for (int y = 0; y < mapY; y++)
		{
			for (int z = -borderWidth; z < mapZ - borderWidth; z++) 
			{
				count += (int)Physics::SphereMap((float)x, (float)y, (float)z)->size();
			}
		}
	}
	return count;
}

int Physics::Border(void)
{
	return borderWidth;
}

void Physics::SafeSphereRelocate(DynamicSphere* sphere, list<DynamicSphere*> *sourceList, list<DynamicSphere*> *destinationList)
{
#if _DEBUG
	int oldLength;	
	oldLength = (int)sourceList->size();
#endif

	sourceList->remove(sphere);

#if _DEBUG
	if (sourceList->size() != oldLength - 1)
	{
		printf("Error! old size %d, after remove: %d", oldLength, sourceList->size());
	}

	oldLength = (int)destinationList->size();
#endif

	destinationList->push_back(sphere);

#if _DEBUG
	if (destinationList->size() != oldLength + 1)
	{
		printf("Error! old size %d, after push: %d", oldLength, destinationList->size());
	}
#endif
}

bool Physics::EnsureOnList(DynamicSphere* sphere, list<DynamicSphere*> *source)
{
	list<DynamicSphere*>::const_iterator sourceIterator = source->begin();
	for (;sourceIterator != source->end();sourceIterator++)
	{
		if (*sourceIterator == sphere) return true;
	}
	return false;
}

void Physics::SetCollisionHandler(SphereSphereCollisionEvent *handler)
{
	collisionHandler = handler;
}

void Physics::RemoveSphere(DynamicSphere *sphere)
{
	spheres.remove(sphere);
	Vector3 position = sphere->Position();
	SphereMap(position.x, position.y, position.z)->remove(sphere);
}